#pragma once
#include <Eigen/Dense>
#include <memory>
#include "discrete_linear_system.h"
using namespace LinearMpc;
namespace KalmanFilter
{
class KF
{
public:
	KF() = delete;
	KF(std::shared_ptr<DiscreteLinearSys> dis_ptr, Eigen::MatrixXd sys_covariance, Eigen::MatrixXd measure_covariance);
	//bool init(Eigen::MatrixXd sys_covariance,Eigen::MatrixXd measure_covariance);
	bool step(Eigen::VectorXd &control_last, Eigen::VectorXd &y);
	bool reset(Eigen::VectorXd state_esti, Eigen::MatrixXd state_cov_esti);
	std::shared_ptr<DiscreteLinearSys> dis_ptr_;
	Eigen::MatrixXd sys_covariance();
	Eigen::MatrixXd measure_covariance();

	Eigen::VectorXd state_predict();
	Eigen::MatrixXd state_cov_predict();
	Eigen::MatrixXd kalman_gain();

	Eigen::VectorXd state_esti();
	Eigen::MatrixXd state_cov_esti();
	Eigen::VectorXd y_error();

	bool set_sys_covariance(Eigen::MatrixXd sys_covariance);
	bool set_measure_covariance(Eigen::MatrixXd measure_covariance);

private:
	unsigned int state_dim_;
	unsigned int control_dim_;
	unsigned int out_dim_;
	Eigen::MatrixXd sys_covariance_;
	Eigen::MatrixXd measure_covariance_;

	Eigen::VectorXd state_predict_;
	Eigen::MatrixXd state_cov_predict_;

	Eigen::MatrixXd kalman_gain_;
	Eigen::VectorXd y_error_;
	Eigen::VectorXd state_esti_;
	Eigen::MatrixXd state_cov_esti_;

	bool set_state_esti(Eigen::VectorXd state_esti);
	bool set_state_cov_esti(Eigen::MatrixXd state_cov_esti);
	bool predict(Eigen::VectorXd &control_last);
	bool update(Eigen::VectorXd &y);
};

} // namespace KalmanFilter